from typing import List, Optional, Tuple
from ctypes import POINTER, c_int, c_char_p, pointer, c_double, c_char

import numpy

from robot_python.robot.robot_lib import RobotLib
from robot_python.robot.data_structure import CommandArg, Speed, Joint, Pose, Tool, Zone, Wobj, DOF, BUFFER_SIZE, CJoint, CPose, CSpeed, CTool, CZone, CWobj, CPayLoad, SdkVersion, \
    MAX_DOF, \
    DataType, PayLoad
from robot_python.util.system import kill_by_port


class Robot:

    def __init__(self):
        """
        机器人初始化
        """
        self.lib = RobotLib()
        self.encoding = "utf-8"

    def system_initialize(self):
        """
        系统初始化
        :return:
        """
        command_arg = CommandArg(c_char_p(bytes("/hanbing", encoding=self.encoding)), 0, 0, 0, 0, 6666, 0)
        fn = self.lib.RobotLib.system_initialize
        fn.argtypes = POINTER(CommandArg),
        fn.restype = c_int
        fn(command_arg)

    def move_start(self):
        """
        机器人上使能
        :return:
        """
        self.lib.RobotInterface.move_start()

    def move_stop(self):
        """
        机器人下使能
        :return:
        """
        self.lib.RobotInterface.move_stop()

    def move_joint(self, joint: Joint, speed: Speed, zone: Zone = None, tool: Tool = None, wobj: Wobj = None) -> None:
        """
        机器人关节运动
        :param joint:
        :param speed:
        :param zone:
        :param tool:
        :param wobj:
        :return:
        """
        self.lib.BaseLib.MoveA(self._c_pointer(joint), self._c_pointer(speed), self._c_pointer(zone), self._c_pointer(tool), self._c_pointer(wobj))

    def move_pose(self, pose: Pose, speed: Speed, zone: Zone = None, tool: Tool = None, wobj: Wobj = None) -> None:
        """
        笛卡尔空间运动
        :param pose:
        :param speed:
        :param zone:
        :param tool:
        :param wobj:
        :return:
        """
        self.lib.BaseLib.MoveJ(self._c_pointer(pose), self._c_pointer(speed), self._c_pointer(zone), self._c_pointer(tool), self._c_pointer(wobj))

    def move_line(self, pose: Pose, speed: Speed, zone: Zone = None, tool: Tool = None, wobj: Wobj = None):
        """
        末端直线运动
        :param pose:
        :param speed:
        :param zone:
        :param tool:
        :param wobj:
        :return:
        """
        self.lib.BaseLib.MoveL(self._c_pointer(pose), self._c_pointer(speed), self._c_pointer(zone), self._c_pointer(tool), self._c_pointer(wobj))

    def move_circle(self, pose: Pose, pose_mid: Pose, speed: Speed, zone: Zone = None, tool: Tool = None, wobj: Wobj = None):
        """
        圆弧运动
        :param wobj: 工件
        :param zone: 转弯区
        :param speed: 速度
        :param pose: 起始位姿
        :param pose_mid: 轨迹中经过的位姿
        :return:
        """
        self.lib.BaseLib.MoveC(self._c_pointer(pose), self._c_pointer(pose_mid), self._c_pointer(speed), self._c_pointer(zone), self._c_pointer(tool), self._c_pointer(wobj))

    def get_data_path(self) -> str:
        """
        获取数据文件夹
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_data_path
        func.restype = POINTER(c_char * BUFFER_SIZE)
        resp = func()
        return str(resp.contents.value, encoding=self.encoding)

    def get_joint(self) -> Joint:
        """
        获取当前关节角度
        :return:
        """
        joint = (c_double * DOF)()
        self.lib.RobotControlerAPI.get_robot_joint(joint)
        return Joint(angle=tuple(joint))

    def get_pose(self) -> Pose:
        """
        获取当前位姿
        :return:
        """
        pose = (c_double * 6)()
        self.lib.RobotControlerAPI.get_robot_cartesian(pose)
        pose = list(pose)
        return Pose(xyz=(pose[0], pose[1], pose[2]), kps=(pose[3], pose[4], pose[5]))

    def get_offset_pose(self, pose: Pose, x: float, y: float, z: float, k: float, p: float, s: float) -> Pose:
        """
        给定一个初始pose和一个offset，计算offset之后的pose
        :param pose:
        :param x: x
        :param y: y
        :param z: z
        :param k: k
        :param p: p
        :param s: s
        :return:
        """
        func = self.lib.BaseLib.Offs
        func.argtypes = (POINTER(CPose), c_double, c_double, c_double, c_double, c_double, c_double)
        func.restype = CPose
        offset_pose = func(self._c_pointer(pose), x, y, z, k, p, s)
        return Pose(xyz=tuple(offset_pose.xyz), kps=tuple(offset_pose.kps))

    def get_tool(self) -> str:
        """
        获取当前的工具
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_tool
        func.restype = POINTER(c_char * BUFFER_SIZE)
        resp = func()
        return str(resp.contents.value, encoding=self.encoding)

    def get_wobj(self) -> str:
        """
        获取当前的工件
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_wobj
        func.restype = POINTER(c_char * BUFFER_SIZE)
        resp = func()
        return str(resp.contents.value, encoding=self.encoding)

    def get_run_status(self) -> int:
        """
        获取运动状态
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_run_state
        return func()

    def get_sdk_version(self) -> str:
        """
        获取SDK版本
        :return:
        """
        func = self.lib.ExtraLib.getSystemVerison
        func.restype = SdkVersion
        resp = func()
        return f"{resp.major}-{resp.minor}-{resp.build}"

    def sleep(self, n):
        """
        机器人休眠
        :param n: 毫秒
        :return:
        """
        self.lib.BaseLib.Rsleep(n)

    def start_project(self, file_name) -> int:
        """
        启动C项目
        :param file_name:
        :return:
        """
        func = self.lib.RobotControlerAPI.start_robot_c_project
        func.argtypes = (POINTER(c_char),)
        func.restype = c_int
        return func(c_char_p(bytes(file_name, encoding=self.encoding)))

    def stop_project(self) -> int:
        """
        停止C项目
        :return:
        """
        func = self.lib.RobotControlerAPI.close_robot_project
        func.restype = c_int
        return func()

    def teach_set_robot_index(self, index) -> int:
        """
        设置要操作的机器人的索引
        :param index:
        :return:
        """
        func = self.lib.RobotControlerAPI.set_robot_index
        func.argtypes = (c_int,)
        func.restype = c_int
        return func(index)

    def teach_get_robot_index(self) -> int:
        """
        获取当前操作的机器人
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_index
        func.argtypes = None
        func.restype = c_int
        return func()

    def teach_set_tool(self, tool_name) -> int:
        """
        设置工具
        :param tool_name:
        :return:
        """
        func = self.lib.RobotControlerAPI.set_robot_tool
        func.argtypes = (POINTER(c_char),)
        func.restype = c_int
        return func(bytes(tool_name, encoding=self.encoding))

    def teach_get_tool(self) -> str:
        """
        获取当前工具
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_tool
        func.argtypes = None
        func.restype = POINTER(c_char * BUFFER_SIZE)
        resp = func()
        return str(resp.contents.value, encoding=self.encoding)

    def teach_set_wobj(self, wobj_name) -> int:
        """
        设置工件
        :param wobj_name:
        :return:
        """
        func = self.lib.RobotControlerAPI.set_robot_wobj
        func.argtypes = (POINTER(c_char),)
        func.restype = c_int
        return func(bytes(wobj_name, encoding=self.encoding))

    def teach_get_wobj(self) -> str:
        """
        获取工件
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_wobj
        func.restype = POINTER(c_char * BUFFER_SIZE)
        resp = func()
        return str(resp.contents.value, encoding=self.encoding)

    def teach_set_coordinate(self, frame: int) -> int:
        """
        设置坐标系
        :param frame:
        :return:
        """
        func = self.lib.RobotControlerAPI.set_robot_teach_coordinate
        func.argtypes = (c_int,)
        func.restype = c_int
        return func(frame)

    def teach_get_coordinate(self) -> int:
        """
        获取坐标系
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_teach_coordinate
        func.argtypes = None
        func.restype = c_int
        return func()

    def teach_get_num(self) -> int:
        """
        获取机器人数量
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_num
        func.argtypes = None
        func.restype = c_int
        return func()

    def teach_get_dof(self) -> int:
        """
        获取DOF
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_dof
        func.argtypes = None
        func.restype = c_int
        return func()

    def teach_get_joint(self) -> Joint:
        """
        获取当前关节
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_joint
        func.argtypes = (POINTER(c_double * MAX_DOF),)
        func.restype = c_int
        joint = (c_double * MAX_DOF)()
        func(joint)
        return Joint(tuple(joint))

    def teach_get_pose(self) -> Pose:
        """
        获取笛卡尔位置
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_cartesian
        func.argtypes = (POINTER(c_double * MAX_DOF),)
        func.restype = c_int
        pose = (c_double * MAX_DOF)()
        func(pose)
        pose = list(pose)
        return Pose(xyz=(pose[0], pose[1], pose[2]), kps=(pose[3], pose[4], pose[5]))

    def teach_set_velocity(self, vel_percent):
        """
        设置示教速度
        :param vel_percent:
        :return:
        """
        func = self.lib.RobotControlerAPI.set_robot_teach_velocity
        func.argtypes = (c_double,)
        func.restype = None
        func(vel_percent)

    def teach_get_velocity(self) -> float:
        """
        获取示教速度
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_teach_velocity
        func.argtypes = None
        func.restype = c_double
        return func()

    def teach_set_run_type(self, is_simulation: bool):
        """
        设置是否开启仿真
        :param is_simulation:
        :return:
        """
        func = self.lib.RobotControlerAPI.set_robot_run_type
        func.argtypes = (c_int,)
        func.restype = None
        func(int(is_simulation))

    def teach_get_run_type(self) -> int:
        """
        获取是否开启仿真
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_run_type
        func.argtypes = None
        func.restype = c_int
        return func()

    def teach_get_run_state(self) -> int:
        """
        获取运行状态
        :return:
        """
        func = self.lib.RobotControlerAPI.get_robot_run_state
        func.argtypes = None
        func.restype = c_int
        return func()

    def teach_clear_move_error(self):
        """
        清除运动错误
        :return:
        """
        func = self.lib.RobotControlerAPI.robot_move_error_clear
        func.argtypes = None
        func.restype = None
        func()

    def teach_move_start(self) -> int:
        """
        使能
        :return:
        """
        func = self.lib.RobotControlerAPI.robot_teach_enable
        func.argtypes = None
        func.restype = c_int
        return func()

    def teach_move_stop(self) -> int:
        """
        下使能
        :return:
        """
        func = self.lib.RobotControlerAPI.robot_teach_disenable
        func.argtypes = None
        func.restype = c_int
        return func()

    def teach_stop(self) -> int:
        """
        停止示教动作
        :return:
        """
        func = self.lib.RobotControlerAPI.robot_teach_stop
        func.argtypes = None
        func.restype = c_int
        return func()

    def teach_move_home(self) -> int:
        """
        示教回零
        :return:
        """
        func = self.lib.RobotControlerAPI.robot_home
        func.argtypes = None
        func.restype = c_int
        return func()

    def teach_move(self, axis: int, direction: int) -> int:
        """
        示教
        :param axis: 轴
        :param direction: 方向
        :return:
        """
        func = self.lib.RobotControlerAPI.robot_teach_move
        func.argtypes = (c_int, c_int)
        func.restype = c_int
        return func(axis, direction)

    def teach_calibrate_axis(self, axis: int) -> int:
        """
        某个轴标零点
        :param axis:
        :return:
        """
        func = self.lib.RobotControlerAPI.calibrate_robot_zero_axis_position
        func.argtypes = (c_int,)
        func.restype = c_int
        return func(axis)

    def teach_calibrate(self) -> int:
        """
        所有轴标零
        :return:
        """
        func = self.lib.RobotControlerAPI.calibrate_robot_zero_position
        func.argtypes = None
        func.restype = c_int
        return func()

    def get_data_num(self, data_type: DataType) -> int:
        """
        获取数据点的个数
        """
        return self.lib.BaseLib.getDataNum(data_type.value)

    def get_data_name(self, data_type: DataType, index: int) -> str:
        """
        获取data_type第index个数据
        """
        data_name = (c_char * BUFFER_SIZE)()
        func = self.lib.BaseLib.getDataName
        func.argtypes = (c_int, c_int, POINTER(c_char))
        func(data_type.value, index, data_name)
        return str(data_name.value, encoding=self.encoding)

    def get_joint_data(self, name: str) -> Joint:
        """
        获取关节数据
        """
        joint = CJoint()
        func = self.lib.BaseLib.getrobjoint
        func.argtypes = (POINTER(c_char), POINTER(CJoint))
        func.restype = c_int
        func(c_char_p(bytes(name, encoding=self.encoding)), pointer(joint))
        return Joint(tuple(joint.angle), int(joint.dof))

    def write_joint_data(self, name: str, joint: Joint):
        """
        写入一个joint数据
        """
        func = self.lib.BaseLib.writerobjoint
        func.argtypes = (POINTER(c_char), POINTER(CJoint))
        func(c_char_p(bytes(name, encoding=self.encoding)), pointer(joint.c))

    def get_pose_data(self, name: str) -> Pose:
        """
        获取位姿数据
        """
        pose = CPose()
        func = self.lib.BaseLib.getrobpose
        func.argtypes = (POINTER(c_char), POINTER(CPose))
        func.restype = c_int
        func(c_char_p(bytes(name, encoding=self.encoding)), pointer(pose))
        return Pose(tuple(pose.xyz), tuple(pose.kps))

    def write_pose_data(self, name: str, pose: Pose):
        """
        写入位姿数据
        """
        func = self.lib.BaseLib.writerobpose
        func.argtypes = (POINTER(c_char), POINTER(CPose))
        func(c_char_p(bytes(name, encoding=self.encoding)), pointer(pose.c))

    def get_speed_data(self, name: str) -> Speed:
        """
        获取速度数据
        """
        speed = CSpeed()
        func = self.lib.BaseLib.getspeed
        func.argtypes = (POINTER(c_char), POINTER(CSpeed))
        func.restype = c_int
        func(c_char_p(bytes(name, encoding=self.encoding)), pointer(speed))
        return Speed(tuple(speed.per), speed.per_flag, speed.tcp, speed.orl, speed.tcp_flag, speed.dof)

    def write_speed_data(self, name: str, speed: Speed):
        """
        写入速度数据
        """
        func = self.lib.BaseLib.writespeed
        func.argtypes = (POINTER(c_char), POINTER(CSpeed))
        func(c_char_p(bytes(name, encoding=self.encoding)), pointer(speed.c))

    def get_zone_data(self, name: str) -> Zone:
        """
        获取转弯区数据
        """
        zone = CZone()
        func = self.lib.BaseLib.getzone
        func.argtypes = (POINTER(c_char), POINTER(CZone))
        func.restype = c_int
        func(c_char_p(bytes(name, encoding=self.encoding)), pointer(zone))
        return Zone(zone.zone_flag, zone.zone_size)

    def set_zone_data(self, name: str, zone: Zone):
        """
        设置转弯区数据
        """
        func = self.lib.BaseLib.writezone
        func.argtypes = (POINTER(c_char), POINTER(CZone))
        func(c_char_p(bytes(name, encoding=self.encoding)), pointer(zone.c))

    def get_tool_data(self, name: str) -> Tool:
        """
        获取工具数据
        """
        tool = CTool()
        func = self.lib.BaseLib.gettool
        func.argtypes = (POINTER(c_char), POINTER(CTool))
        func.restype = c_int
        func(c_char_p(bytes(name, encoding=self.encoding)), pointer(tool))
        tframe = Pose(tuple(tool.tframe.xyz), tuple(tool.tframe.kps))
        payload = PayLoad(tool.payload.m, tuple(tool.payload.cm), tuple(tool.payload.I), tuple(tool.payload.I2))
        return Tool(tool.robhold, tframe, payload)

    def write_tool_data(self, name: str, tool: Tool):
        """
        写入工具数据
        """
        func = self.lib.BaseLib.writetool
        func.argtypes = (POINTER(c_char), POINTER(CTool))
        func(c_char_p(bytes(name, encoding=self.encoding)), pointer(tool.c))

    def get_wobj_data(self, name: str) -> Wobj:
        """
        获取工件数据
        """
        wobj = CWobj()
        func = self.lib.BaseLib.getwobj
        func.argtypes = (POINTER(c_char), POINTER(CWobj))
        func.restype = c_int
        func(c_char_p(bytes(name, encoding=self.encoding)), pointer(wobj))
        uframe = Pose(tuple(wobj.uframe.xyz), tuple(wobj.uframe.kps))
        oframe = Pose(tuple(wobj.oframe.xyz), tuple(wobj.oframe.kps))
        return Wobj(wobj.robhold, wobj.ufprog, wobj.ufmec, uframe, oframe)

    def write_wobj_data(self, name: str, wobj: Wobj):
        """
        写入工件数据
        """
        func = self.lib.BaseLib.writewobj
        func.argtypes = (POINTER(c_char), POINTER(CWobj))
        func.restype = c_int
        func(c_char_p(bytes(name, encoding=self.encoding)), pointer(wobj.c))

    def get_payload_data(self, name: str) -> PayLoad:
        """
        获取负载
        """
        payload = CPayLoad()
        func = self.lib.DynamicsLib.getPayLoad
        func.argtypes = (POINTER(CPayLoad), POINTER(c_char))
        func.restype = c_int
        func(pointer(payload), bytes(name, encoding=self.encoding))
        return PayLoad(payload.m, tuple(payload.cm), tuple(numpy.array(payload.I).flatten()), tuple(numpy.array(payload.I2).flatten()))

    def get_user_data(self, name: str) -> Optional[Pose]:
        """
        获取用户坐标系
        """
        pose = CPose()
        func = self.lib.BaseLib.getuser
        func.argtypes = (POINTER(c_char), POINTER(CPose))
        func.restype = c_int
        resp = func(c_char_p(bytes(name, encoding=self.encoding)), pointer(pose))
        if resp != 0:
            return None
        return Pose(tuple(pose.xyz), tuple(pose.kps))

    def calibrate_tool_data(self, data_index: int) -> int:
        """
        采集工具标定数据
        """
        func = self.lib.RobotControlerAPI.calibration_tool_data
        func.argtypes = (c_int,)
        func.restype = c_int
        return func(data_index)

    def calibrate_tool(self, robhold: bool, data_num: int, payload_name: str, tool_name: str) -> Tuple[int, Optional[Tool]]:
        """
        标定工具
        """
        tool = CTool()
        func = self.lib.RobotControlerAPI.calibration_tool
        func.argtypes = (c_int, c_int, POINTER(c_char), POINTER(c_char), POINTER(CTool))
        func.restype = c_int
        resp = func(int(robhold), data_num, bytes(payload_name, encoding=self.encoding), bytes(tool_name, encoding=self.encoding), tool)
        if resp < 0:
            return resp, None
        pose = Pose(tuple(tool.tframe.xyz), tuple(tool.tframe.kps))
        payload = PayLoad(tool.payload.m, tuple(tool.payload.cm), tuple(numpy.array(tool.payload.I).flatten()), tuple(numpy.array(tool.payload.I2).flatten()))
        return resp, Tool(tool.robhold, pose, payload)

    def calibrate_wobj_data(self, data_index: int) -> int:
        """
        采集工件标定数据
        """
        func = self.lib.RobotControlerAPI.calibration_wobj_data
        func.argtypes = (c_int,)
        func.restype = c_int
        return func(data_index)

    def calibrate_wobj(self, robhold: bool, ufmec: int, ufprog: int, user_name: str, wobj_name: str) -> Tuple[int, Optional[Wobj]]:
        """
        标定工件
        """
        wobj = CWobj()
        func = self.lib.RobotControlerAPI.calibration_wobj
        func.argtypes = (c_int, c_int, c_int, POINTER(c_char), POINTER(c_char), POINTER(CWobj))
        func.restype = c_int
        resp = func(int(robhold), ufmec, ufprog, c_char_p(bytes(user_name, encoding=self.encoding)), c_char_p(bytes(wobj_name, encoding=self.encoding)), pointer(wobj))
        if resp != 0:
            return resp, None
        uframe = Pose(tuple(wobj.uframe.xyz), tuple(wobj.uframe.kps))
        oframe = Pose(tuple(wobj.oframe.xyz), tuple(wobj.oframe.kps))
        return resp, Wobj(wobj.robhold, wobj.ufprog, wobj.ufmec, uframe, oframe)

    def calibrate_user_data(self, data_index) -> int:
        """
        标定用户坐标系数据
        """
        func = self.lib.RobotControlerAPI.calibration_user_data
        func.argtypes = (c_int,)
        func.restype = c_int
        return func(data_index)

    def calibrate_user(self, name: str) -> Tuple[int, Optional[Pose]]:
        """
        标定用户坐标系
        """
        pose = CPose()
        func = self.lib.RobotControlerAPI.calibration_user
        func.argtypes = (POINTER(c_char), POINTER(CPose))
        func.restype = c_int
        resp = func(c_char_p(bytes(name, encoding=self.encoding)), pose)
        if resp != 0:
            return resp, None
        return resp, Pose(tuple(pose.xyz), tuple(pose.kps))

    def calibrate_dynamics_data(self) -> int:
        """
        采集动力学辨识数据
        """
        func = self.lib.RobotControlerAPI.identification_dynamics_data
        func.restype = c_int
        return func()

    def calibrate_dynamics(self) -> int:
        """
        动力学辨识
        """
        func = self.lib.RobotControlerAPI.identification_dynamics
        func.restype = c_int
        return func()

    def calibrate_payload_data(self) -> int:
        """
        辨识负载数据
        """
        func = self.lib.RobotControlerAPI.identification_payload_dynamics_data
        func.restype = c_int
        return func()

    def calibrate_nopayload_data(self) -> int:
        """
        辨识无负载数据
        """
        func = self.lib.RobotControlerAPI.identification_nopayload_dynamics_data
        func.restype = c_int
        return func()

    def calibrate_payload(self, name: str) -> int:
        """
        负载辨识
        """
        func = self.lib.RobotControlerAPI.identification_payload_dynamics
        func.argtypes = (POINTER(c_char),)
        func.restype = c_int
        return func(c_char_p(bytes(name, encoding=self.encoding)))

    def get_dynamics_identification_state(self) -> int:
        """
        获取动力学辨识状态
        0:未运行辨识；1:采集动力学辨识数据；2:动力学辨识；3:采集负载辨识带负载数据；4:采集负载辨识无负载数据；5:负载辨识
        """
        func = self.lib.RobotControlerAPI.get_dynamics_identification_state
        func.restype = c_int
        return func()

    @staticmethod
    def _c_pointer(data):
        """
        获取C结构体
        :param data:
        :return:
        """
        return pointer(data.c) if data else None


if __name__ == '__main__':
    kill_by_port(6666)
    robot = Robot()
    robot.system_initialize()
    # print(robot.get_joint_data("j1").to_json())
    # print(robot.get_user_data("wobjuserframe1").to_json())
    # print(robot.get_data_name(DataType.USER, 1))
    data = {}

    robot.get_payload_data()
    robot.w
    import json
    for i in range(2):
        data_name = robot.get_data_name(DataType.JOINT, i)
        data[data_name] = json.loads(json.dumps(robot.get_joint_data(data_name).to_json()).replace("(", "[").replace(")",  "]"))
    print(data)
    # print(robot.get_data_num(DataType.PAYLOAD))
    # a = robot.get_payload_data("tool0").to_json()
    # import json
    # print(json.dumps(a))
    # robot.move_start()
    # robot.calibrate_dynamics_data()
    # import time
    # while True:
    #     print(robot.get_dynamics_identification_state())
    #     time.sleep(1)
    # print(robot.get_dynamics_identification_state())

    # tool_count = robot.get_data_num(DataType.TOOL)
    # print(f"工具个数{tool_count}")
    # for i in range(tool_count):
    #     data_name = robot.get_data_name(DataType.TOOL, i)
    #     print(data_name)
    #
    # robot.calibrate_tool_data(0)
    # robot.calibrate_tool_data(1)
    # robot.calibrate_tool_data(2)
    # name = "yujiannan1"
    # print(f"获取工具{robot.get_tool_data(name)}")
    # print(robot.calibrate_tool(True, 3, "payload0", name))
    # print(f"获取工具{robot.get_tool_data(name)}")
    #
    # exit()
    # for i in range(6):
    #     print(robot.calibrate_user_data(i))
    # robot.calibrate_wobj_data(0)
    # robot.calibrate_wobj_data(1)
    # robot.calibrate_wobj_data(2)
    # print(robot.calibrate_wobj(True, 0, 0, "userframe666", "yujiannan"))

    # user_count = robot.get_data_num(DataType.USER)
    # for i in range(user_count):
    #     print(robot.get_data_name(DataType.USER, i))
    # robot.calibrate_user_data(0)
    # robot.calibrate_user_data(1)
    # robot.calibrate_user_data(2)
    # print(robot.calibrate_user("yujiannan"))
    # user_count = robot.get_data_num(DataType.USER)
    # for i in range(user_count):
    #     print(robot.get_data_name(DataType.USER, i))

    # wobj = robot.get_wobj_data("wobj666")
    # print(wobj.to_string())
